import sympy
import sympybotics

rbtdef = sympybotics.RobotDef('leg2dof', # robot name
		  [('0', 0.316, 0, 'q'),
		   ('0', 0.319, 0, 'q'),
		   ], # (alpha, a, d, theta)
		    dh_convention='standard' # either 'standard' or 'modified'
		      )
#rbtdef.frictionmodel = {'Coulomb', 'viscous'} # options are None or a combination of 'Coulomb', 'viscous' and 'offset'
rbtdef.dynparms()
rbt = sympybotics.RobotDynCode(rbtdef, verbose=True)
tau_str = sympybotics.robotcodegen.robot_code_to_func('C', rbt.invdyn_code, 'tau_out', 'tau', rbtdef)
#print(tau_str)
f = open('leg2dof_tau_out.cpp','w')
f.write(tau_str)
f.close()
rbt.calc_base_parms()

regressor_str = sympybotics.robotcodegen.robot_code_to_func('jl', rbt.H_code, 'H', 'regressor_func', rbtdef)
#print(regressor_str)
f = open('leg2dof_regressor.cpp','w')
f.write(regressor_str)
f.close()

f = open('leg2dof_base_para.txt','w')
print(rbt.dyn.n_dynparms)
f.write('the nunber of all parameters is: \n')
f.write(str(rbt.dyn.n_dynparms))
f.write('\n')
f.write('\n')
print(rbt.dyn.dynparms)
f.write('all the parameters is: \n')
f.write(str(rbt.dyn.dynparms))
f.write('\n')
f.write('\n')
print(rbt.dyn.baseparms)
f.write('the base parameters is: \n')
f.write(str(rbt.dyn.baseparms))
f.write('\n')
f.write('\n')
print(rbt.dyn.base_idxs)
f.write('the index of base parameters is: \n')
f.write(str(rbt.dyn.base_idxs))
f.write('\n')
f.write('\n')
print(rbt.dyn.Pb)
f.write('the matrix Pb is: \n')
f.write(str(rbt.dyn.Pb))
f.write('\n')
f.write('\n')
print(rbt.dyn.Pd)
f.write('the matrix Pd is: \n')
f.write(str(rbt.dyn.Pd))
f.write('\n')
f.write('\n')
print(rbt.dyn.Kd)
f.write('the matrix Kd is: \n')
f.write(str(rbt.dyn.Kd))
f.write('\n')
f.write('\n')
f.close
